﻿
package bsplib;
import bsplib.BspTriangle;
import bsplib.BspVertex;
import flash.Memory;
import flash.sampler.NewObjectSample;
import flash.utils.ByteArray;
import haxe.Log;

class Projector 
{

    public var m : Matrix4A;
    public var focusDistance : Float;
    public var near : Float;
    
    private var eyePoint : Vec3f;
        
    public function new( m : Matrix4A )
    {
        focusDistance = 1000;
        near = 500;
        this.m = m;
        var im = m.clone().invert();
        eyePoint = new Vec3f( im.m41, im.m42, im.m43 );
    }
    
    public inline function projectCoords( x : Float, y : Float, z : Float, dst : Vec3f ) : Void {
        dst.z = x * m.m13 + y * m.m23 + z * m.m33 + m.m43;
        var zInv = focusDistance / dst.z;
        dst.x = (x * m.m11 + y * m.m21 + z * m.m31 + m.m41) * zInv;
        dst.y = (x * m.m12 + y * m.m22 + z * m.m32 + m.m42) * zInv;
    }
    
    public inline function projectVecList( src : flash.Vector < Float > , dstVerts : flash.Vector < Float > ) : Void {
        var sindex = 0;
        var dindex = 0;
        var n = src.length;

        while ( sindex < n ) {
            var srcx = src[sindex++]; var srcy = src[sindex++]; var srcz = src[sindex++];
            var zInv = focusDistance / (srcx * m.m13 + srcy * m.m23 + srcz * m.m33 + m.m43); // TODO: decide if this is negated
            dstVerts[dindex++] = (srcx * m.m11 + srcy * m.m21 + srcz * m.m31 + m.m41) * zInv;
            dstVerts[dindex++] = (srcx * m.m12 + srcy * m.m22 + srcz * m.m32 + m.m42) * zInv;
        }
    }
    
    public inline function projectVecList2( srcVerts : flash.Vector<Vec3f>, dstVerts : flash.Vector<Float> ) : Void {
        var sindex = 0;
        var dindex = 0;
        var n = srcVerts.length;
        var src;
        while ( sindex < n ) {
            src = srcVerts[sindex++];
            var zInv = focusDistance / (src.x * m.m13 + src.y * m.m23 + src.z * m.m33 + m.m43); // TODO: decide if this is negated
            dstVerts[dindex++] = (src.x * m.m11 + src.y * m.m21 + src.z * m.m31 + m.m41) * zInv;
            dstVerts[dindex++] = (src.x * m.m12 + src.y * m.m22 + src.z * m.m32 + m.m42) * zInv;
        }
    }
    
    private static inline var SizeOfDouble : Int = 8;
    private static inline var SizeOfFloat : Int = 4;
    
    public inline function projectVecList3( srcVerts : ByteArray, dstVerts : flash.Vector < Float > ) : Void {
        var sindex = 0;
        var dindex = 0;
        var n = srcVerts.length;
        
        Memory.select( srcVerts );
        
        while ( sindex < n ) {
            var srcx = Memory.getDouble( sindex );
            sindex += SizeOfDouble;
            var srcy = Memory.getDouble( sindex );
            sindex += SizeOfDouble;
            var srcz = Memory.getDouble( sindex );
            sindex += SizeOfDouble;
            
            var zInv = focusDistance / (srcx * m.m13 + srcy * m.m23 + srcz * m.m33 + m.m43); // TODO: decide if this is negated
            dstVerts[dindex++] = (srcx * m.m11 + srcy * m.m21 + srcz * m.m31 + m.m41) * zInv;
            dstVerts[dindex++] = (srcx * m.m12 + srcy * m.m22 + srcz * m.m32 + m.m42) * zInv;
        }
    }
    
    public inline function projectVecList4( srcVerts : ByteArray, dstVerts : flash.Vector < Float > ) : Void {
        var sindex = 0;
        var dindex = 0;
        var n = srcVerts.length;
        
        Memory.select( srcVerts );
        
        while ( sindex < n ) {
            var srcx = Memory.getFloat( sindex );
            sindex += SizeOfFloat;
            var srcy = Memory.getFloat( sindex );
            sindex += SizeOfFloat;
            var srcz = Memory.getFloat( sindex );
            sindex += SizeOfFloat;
            
            var zInv = focusDistance / (srcx * m.m13 + srcy * m.m23 + srcz * m.m33 + m.m43); // TODO: decide if this is negated
            dstVerts[dindex++] = (srcx * m.m11 + srcy * m.m21 + srcz * m.m31 + m.m41) * zInv;
            dstVerts[dindex++] = (srcx * m.m12 + srcy * m.m22 + srcz * m.m32 + m.m42) * zInv;
        }
    }
    
    public inline function projectVecList5( srcVerts : flash.Vector<BspVertex>, dstVerts : flash.Vector<Float> ) : Void {
        var sindex = 0;
        var dindex = 0;
        var n = srcVerts.length;
        var src;
        while ( sindex < n ) {
            src = srcVerts[sindex++].pos;
            var zInv = focusDistance / (src.x * m.m13 + src.y * m.m23 + src.z * m.m33 + m.m43); // TODO: decide if this is negated
            dstVerts[dindex++] = (src.x * m.m11 + src.y * m.m21 + src.z * m.m31 + m.m41) * zInv;
            dstVerts[dindex++] = (src.x * m.m12 + src.y * m.m22 + src.z * m.m32 + m.m42) * zInv;
        }
    }
    
    public function projectAndNearClip(
                                srcVerts : flash.Vector<BspVertex>,
                                srcTris : flash.Vector<BspTriangle>,
                                dstVerts : flash.Vector<Float>,
                                dstUVs : flash.Vector<Float>,
                                dstIndexes : flash.Vector<Int>
                            ) : Void
    {
        var destVertIndex = 0;
        var destUVIndex = 0;
        for ( srcVert in srcVerts ) {
            var pos = srcVert.pos;
            srcVert.screenZ = (pos.x * m.m13 + pos.y * m.m23 + pos.z * m.m33 + m.m43);
            
            if ( srcVert.screenZ > near ) {
                var zInv = focusDistance / srcVert.screenZ;
                dstVerts[destVertIndex++] = (pos.x * m.m11 + pos.y * m.m21 + pos.z * m.m31 + m.m41) * zInv;
                dstVerts[destVertIndex++] = (pos.x * m.m12 + pos.y * m.m22 + pos.z * m.m32 + m.m42) * zInv;
                
                dstUVs[destUVIndex++] = srcVert.uv.x;
                dstUVs[destUVIndex++] = srcVert.uv.y;
                dstUVs[destUVIndex++] = zInv;
            }else {
                destVertIndex += 2;
                destUVIndex += 3;
            }
        }
        
        var destTriIndex = 0;
        for ( tri in srcTris ) {
            var v0 = tri.v0;
            var v1 = tri.v1;
            var v2 = tri.v2;

            var c0 = v0.screenZ > near;
            var c1 = v1.screenZ > near;
            var c2 = v2.screenZ > near;
            
            if ( !c0 && !c1 && !c2 ) continue;
            if ( c0 && c1 && c2 ) {
                dstIndexes[destTriIndex++] = v0.index;
                dstIndexes[destTriIndex++] = v1.index;
                dstIndexes[destTriIndex++] = v2.index;
            }else { // At least one vertex is invisible and at least one is visible
                var resultIsSingleTriangle : Bool;
                var vSwap;
                if ( c0 ) {
                    if ( !c1 && !c2 ) {
                        // v0 is visible, v1 and v2 are not
                        // no reorder needed
                        resultIsSingleTriangle = true;
                    }else if ( c2 ) {
                        // v0 and v2 are visible, v1 is not
                        vSwap = v0; v0 = v1; v1 = v2; v2 = vSwap;
                        resultIsSingleTriangle = false;
                    }else {
                        // v0 and v1 are visible, v2 is not
                        vSwap = v2; v2 = v1; v1 = v0; v0 = vSwap;
                        resultIsSingleTriangle = false;
                    }
                }else {
                    if ( c1 && c2 ) {
                        // v1 and v2 are visible, v0 is not
                        // no reorder needed
                        resultIsSingleTriangle = false;
                    }else if ( c1 ) {
                        // v1 is visible, v0 and v2 are not
                        vSwap = v0; v0 = v1; v1 = v2; v2 = vSwap;
                        resultIsSingleTriangle = true;
                    }else {
                        // v2 is visible, v0 and v1 are not
                        vSwap = v2; v2 = v1; v1 = v0; v0 = vSwap;
                        resultIsSingleTriangle = true;
                    }
                }
                
                
                var newVertsIdx = Std.int( destVertIndex / 2 );
                
                var f0 = v0.screenZ - near;
                var f1 = v1.screenZ - near;
                var f2 = v2.screenZ - near;
                
                var npos : Vec3f = new Vec3f(0, 0, 0); // TODO: cache this as a private member
                
                var zInv = focusDistance / near; // TODO: cache this as a private member
                
                var factor = -f1 / (f0 - f1);
                npos.cInterpolate( v1.pos, v0.pos, factor ); 
                
                dstVerts[destVertIndex++] = (npos.x * m.m11 + npos.y * m.m21 + npos.z * m.m31 + m.m41) * zInv;
                dstVerts[destVertIndex++] = (npos.x * m.m12 + npos.y * m.m22 + npos.z * m.m32 + m.m42) * zInv;
                
                dstUVs[destUVIndex++] = v1.uv.x + ( v0.uv.x - v1.uv.x ) * factor;
                dstUVs[destUVIndex++] = v1.uv.y + ( v0.uv.y - v1.uv.y ) * factor;
                dstUVs[destUVIndex++] = zInv;
                
                factor = -f2 / (f0 - f2);
                
                npos.cInterpolate( v2.pos, v0.pos, factor );
                
                dstVerts[destVertIndex++] = (npos.x * m.m11 + npos.y * m.m21 + npos.z * m.m31 + m.m41) * zInv;
                dstVerts[destVertIndex++] = (npos.x * m.m12 + npos.y * m.m22 + npos.z * m.m32 + m.m42) * zInv;
                
                dstUVs[destUVIndex++] = v2.uv.x + ( v0.uv.x - v2.uv.x ) * factor;
                dstUVs[destUVIndex++] = v2.uv.y + ( v0.uv.y - v2.uv.y ) * factor;
                dstUVs[destUVIndex++] = zInv;
                
                if ( resultIsSingleTriangle ) {
                    dstIndexes[destTriIndex++] = v0.index;
                    dstIndexes[destTriIndex++] = newVertsIdx;
                    dstIndexes[destTriIndex++] = newVertsIdx + 1;
                }else {
                    dstIndexes[destTriIndex++] = newVertsIdx;
                    dstIndexes[destTriIndex++] = v1.index;
                    dstIndexes[destTriIndex++] = v2.index;
                    
                    dstIndexes[destTriIndex++] = newVertsIdx;
                    dstIndexes[destTriIndex++] = v2.index;
                    dstIndexes[destTriIndex++] = newVertsIdx + 1;
                }
                
            }
            
        }
        
        dstIndexes.length = destTriIndex;
        Debug.assert( destTriIndex % 3 == 0 );
        
    }
    
    public function projectAndNearClip2(
                                srcVerts : flash.Vector<BspVertex>,
                                srcTris : flash.Vector<BspTriangle>,
                                dstVerts : flash.Vector<Float>,
                                dstUVs : flash.Vector<Float>,
                                dstLMUVs : flash.Vector<Float>,
                                dstIndexes : flash.Vector<Int>
                            ) : Void
    {
        var destVertIndex = 0;
        var destUVIndex = 0;
        for ( srcVert in srcVerts ) {
            var pos = srcVert.pos;
            srcVert.screenZ = (pos.x * m.m13 + pos.y * m.m23 + pos.z * m.m33 + m.m43);
            
            if ( srcVert.screenZ > near ) {
                var zInv = focusDistance / srcVert.screenZ;
                dstVerts[destVertIndex++] = (pos.x * m.m11 + pos.y * m.m21 + pos.z * m.m31 + m.m41) * zInv;
                dstVerts[destVertIndex++] = (pos.x * m.m12 + pos.y * m.m22 + pos.z * m.m32 + m.m42) * zInv;
                
                // TODO: Cache this:
                dstLMUVs[destUVIndex] = srcVert.lmuv.x;
                dstUVs[destUVIndex++] = srcVert.uv.x;
                dstLMUVs[destUVIndex] = srcVert.lmuv.y;
                dstUVs[destUVIndex++] = srcVert.uv.y;
                dstLMUVs[destUVIndex] = zInv;
                dstUVs[destUVIndex++] = zInv;
            }else {
                destVertIndex += 2;
                destUVIndex += 3;
            }
        }
        
        var destTriIndex = 0;
        for ( tri in srcTris ) {
            var v0 = tri.v0;
            var v1 = tri.v1;
            var v2 = tri.v2;

            var c0 = v0.screenZ > near;
            var c1 = v1.screenZ > near;
            var c2 = v2.screenZ > near;
            
            if ( !c0 && !c1 && !c2 ) continue;
            if ( c0 && c1 && c2 ) {
                dstIndexes[destTriIndex++] = v0.index;
                dstIndexes[destTriIndex++] = v1.index;
                dstIndexes[destTriIndex++] = v2.index;
            }else { // At least one vertex is invisible and at least one is visible
                var resultIsSingleTriangle : Bool;
                var vSwap;
                if ( c0 ) {
                    if ( !c1 && !c2 ) {
                        // v0 is visible, v1 and v2 are not
                        // no reorder needed
                        resultIsSingleTriangle = true;
                    }else if ( c2 ) {
                        // v0 and v2 are visible, v1 is not
                        vSwap = v0; v0 = v1; v1 = v2; v2 = vSwap;
                        resultIsSingleTriangle = false;
                    }else {
                        // v0 and v1 are visible, v2 is not
                        vSwap = v2; v2 = v1; v1 = v0; v0 = vSwap;
                        resultIsSingleTriangle = false;
                    }
                }else {
                    if ( c1 && c2 ) {
                        // v1 and v2 are visible, v0 is not
                        // no reorder needed
                        resultIsSingleTriangle = false;
                    }else if ( c1 ) {
                        // v1 is visible, v0 and v2 are not
                        vSwap = v0; v0 = v1; v1 = v2; v2 = vSwap;
                        resultIsSingleTriangle = true;
                    }else {
                        // v2 is visible, v0 and v1 are not
                        vSwap = v2; v2 = v1; v1 = v0; v0 = vSwap;
                        resultIsSingleTriangle = true;
                    }
                }
                
                
                var newVertsIdx = Std.int( destVertIndex / 2 );
                
                var f0 = v0.screenZ - near;
                var f1 = v1.screenZ - near;
                var f2 = v2.screenZ - near;
                
                var npos : Vec3f = new Vec3f(0, 0, 0); // TODO: cache this as a private member
                
                var zInv = focusDistance / near; // TODO: cache this as a private member
                
                var factor = -f1 / (f0 - f1);
                npos.cInterpolate( v1.pos, v0.pos, factor ); 
                
                dstVerts[destVertIndex++] = (npos.x * m.m11 + npos.y * m.m21 + npos.z * m.m31 + m.m41) * zInv;
                dstVerts[destVertIndex++] = (npos.x * m.m12 + npos.y * m.m22 + npos.z * m.m32 + m.m42) * zInv;
                
                dstLMUVs[destUVIndex] = v1.lmuv.x + ( v0.lmuv.x - v1.lmuv.x ) * factor;
                dstUVs[destUVIndex++] = v1.uv.x + ( v0.uv.x - v1.uv.x ) * factor;
                dstLMUVs[destUVIndex] = v1.lmuv.y + ( v0.lmuv.y - v1.lmuv.y ) * factor;
                dstUVs[destUVIndex++] = v1.uv.y + ( v0.uv.y - v1.uv.y ) * factor;
                dstLMUVs[destUVIndex] = zInv;
                dstUVs[destUVIndex++] = zInv;
                
                factor = -f2 / (f0 - f2);
                
                npos.cInterpolate( v2.pos, v0.pos, factor );
                
                dstVerts[destVertIndex++] = (npos.x * m.m11 + npos.y * m.m21 + npos.z * m.m31 + m.m41) * zInv;
                dstVerts[destVertIndex++] = (npos.x * m.m12 + npos.y * m.m22 + npos.z * m.m32 + m.m42) * zInv;
                
                dstLMUVs[destUVIndex] = v2.lmuv.x + ( v0.lmuv.x - v2.lmuv.x ) * factor;
                dstUVs[destUVIndex++] = v2.uv.x + ( v0.uv.x - v2.uv.x ) * factor;
                dstLMUVs[destUVIndex] = v2.lmuv.y + ( v0.lmuv.y - v2.lmuv.y ) * factor;
                dstUVs[destUVIndex++] = v2.uv.y + ( v0.uv.y - v2.uv.y ) * factor;
                dstLMUVs[destUVIndex] = zInv;
                dstUVs[destUVIndex++] = zInv;
                
                if ( resultIsSingleTriangle ) {
                    dstIndexes[destTriIndex++] = v0.index;
                    dstIndexes[destTriIndex++] = newVertsIdx;
                    dstIndexes[destTriIndex++] = newVertsIdx + 1;
                }else {
                    dstIndexes[destTriIndex++] = newVertsIdx;
                    dstIndexes[destTriIndex++] = v1.index;
                    dstIndexes[destTriIndex++] = v2.index;
                    
                    dstIndexes[destTriIndex++] = newVertsIdx;
                    dstIndexes[destTriIndex++] = v2.index;
                    dstIndexes[destTriIndex++] = newVertsIdx + 1;
                }
                
            }
            
        }
        
        dstIndexes.length = destTriIndex;
        Debug.assert( destTriIndex % 3 == 0 );
        
    }
    
    public inline function cullPlane( planeNormal : Vec3f, dist : Float ) : Bool {
        return (planeNormal.dot(eyePoint) - dist) > 0;
    }
    
    
    private static inline var XUP = 1;
    private static inline var XMID = 2;
    private static inline var XDOWN = 4;
    private static inline var YUP = 8;
    private static inline var YMID = 16;
    private static inline var YDOWN = 32;
    private static inline var FRONT = 64;
    private static inline var INSIDE = FRONT | XMID | YMID;
    private static inline var XSIDES = XUP | XDOWN;
    private static inline var YSIDES = YUP | YDOWN;
    
    // TODO: Improve this pathetic excuse of a culling algorithm
    public function cullAABox( mins : Vec3f, maxs : Vec3f ) : Bool {
        var tmp : Vec3f = new Vec3f(0,0,0);
        var dirs : Int = 0;

        dirs |= cullAABoxHelper( mins.x, mins.y, mins.z, tmp );
        if ( dirs & INSIDE == INSIDE ) return true;
        dirs |= cullAABoxHelper( mins.x, mins.y, maxs.z, tmp );
        if ( dirs & INSIDE == INSIDE ) return true;
        dirs |= cullAABoxHelper( mins.x, maxs.y, mins.z, tmp );
        if ( dirs & INSIDE == INSIDE ) return true;
        dirs |= cullAABoxHelper( mins.x, maxs.y, maxs.z, tmp );
        if ( dirs & INSIDE == INSIDE ) return true;
        dirs |= cullAABoxHelper( maxs.x, mins.y, mins.z, tmp );
        if ( dirs & INSIDE == INSIDE ) return true;
        dirs |= cullAABoxHelper( maxs.x, mins.y, maxs.z, tmp );
        if ( dirs & INSIDE == INSIDE ) return true;
        dirs |= cullAABoxHelper( maxs.x, maxs.y, mins.z, tmp );
        if ( dirs & INSIDE == INSIDE ) return true;
        dirs |= cullAABoxHelper( maxs.x, maxs.y, maxs.z, tmp );
        if ( dirs & INSIDE == INSIDE ) return true;
        
        if ( dirs & FRONT == 0 ) return false;
        if ( ( dirs & XMID == 0 ) && ( dirs & XSIDES != XSIDES ) ) return false;
        if ( ( dirs & YMID == 0 ) && ( dirs & YSIDES != YSIDES ) ) return false;
        
        return true;
    }
    
    private inline function cullAABoxHelper( x : Float, y : Float, z : Float, tmp : Vec3f ) : Int {
        var dirs : Int = 0;
        var viewWidth : Float = 500;
        var viewHeight : Float = 400;
        projectCoords( x, y, z, tmp );
        if ( tmp.z > 0 ) {
            dirs |= FRONT;
            
            if ( tmp.x < -viewWidth ) dirs |= XDOWN;
            else if ( tmp.x > viewWidth ) dirs |= XUP;
            else dirs |= XMID;
            
            if ( tmp.y < -viewHeight ) dirs |= YDOWN;
            else if ( tmp.y > viewHeight ) dirs |= YUP;
            else dirs |= YMID;
        }else {
            if ( -tmp.x < -viewWidth ) dirs |= XDOWN;
            else if ( -tmp.x > viewWidth ) dirs |= XUP;
            else dirs |= XMID;
            
            if ( -tmp.y < -viewHeight) dirs |= YDOWN;
            else if ( -tmp.y > viewHeight ) dirs |= YUP;
            else dirs |= YMID;
        }
        
        return dirs;
    }
}
